#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Ultrasonic,     sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     MiddleMotor,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     DCArmLift,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     RightMotor,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    WristLift,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  motor[MiddleMotor]=0;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  motor[MiddleMotor] = 0;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  motor[MiddleMotor] = 0;
  wait1Msec(duration);
}


void Drift (int power, int duration)
{
	motor[LeftMotor]= 0;
	motor[RightMotor]= 0;
	motor[MiddleMotor]= power;
	wait1Msec(duration);
}

void StopMotor ()
{
	motor[LeftMotor]= 0;
	motor[RightMotor]= 0;
	motor[MiddleMotor]= 0;
}


void ArmProgram()
{
	motor[LeftMotor] = 0;
	motor[RightMotor] = 0;
	motor[MiddleMotor] = 0;
	//servo[WristLift] = 0;	//Just for testing purposes
	motor[DCArmLift] = -25;
	wait1Msec(4000);
	servo[WristLift] = 150;
	motor[DCArmLift] = 0;
	wait1Msec(500);					//Added Recently
	//wait1Msec(2000);
	motor[DCArmLift] = 25;
	wait1Msec(450);
	motor[DCArmLift] = 6;
	wait1Msec(2000);
	Forward(100,600);
	Forward(0,10);
	motor[DCArmLift] = -15;
	wait1Msec(500);
	Backward(100,1000);
}

void initializeRobot ()
{
	servo[WristLift] = 250;
	motor[LeftMotor] = 0;
	motor[RightMotor] = 0;
	motor[MiddleMotor] = 0;
	motor[DCArmLift] = 0;
}

task main()
{
	waitForStart();

	initializeRobot();

	//Forward(100,1000);
	Forward(20,200);
	Forward(40,200);
	Forward(60,100);
	Forward(80,350);
	Forward(60,100);
	Forward(40,300);
	Forward(20,300);

	if(SensorValue[IR1200]==5)
	{
		Forward(0,500);			//Added Recently
		//Backward(100,500);	//Added Recently
		//Forward(0,500);			//Added Recently
		ArmProgram();
		Backward(100,750);
		turnL(50,500);
		StopMotor();
		Drift(100,750);
		StopMotor();
	}

	else if(SensorValue[IR1200]!=5)
	{
		Forward(0,500);			//Added Recently
		Backward(30,200);
		Backward(100,600);
		Backward(30,200);
		Forward(0,500);
		Drift(-100,1900);
		Forward(0,500);
		//Forward(100,750);
		Forward(30,200);
		Forward(100,500);	//Added Recently
		Forward(30,200);
		Forward(0,500);			//Added Recently
		//Backward(100,500);	//Added Recently
		//Forward(0,500);			//Added Recently
		ArmProgram();
		Backward(100,250);
		Drift(100,1000);
		StopMotor();
		turnL(50,1000);
		StopMotor();
	}
}
